﻿#include <gtest/gtest.h>

#include "math/linear_math.h"
#include "math/quaternion.h"

#include <Dense>
#include <ctime>
#include <random>

#include "dbg.h"

using namespace CAGD;

// 测试 Quaternion
TEST(ATestQuaternion, BasisFunctions)
{
    {
        Quaternion qa(2, {-2, 3, -4});
        Quaternion qb(1, {-2, 5, -6});
        EXPECT_TRUE((qa + qb) == Quaternion(3, {-4, 8, -10}));
        EXPECT_TRUE((qa - qb) == Quaternion(1, {0, -2, 2}));
        EXPECT_TRUE((qa * 2) == Quaternion(4, {-4, 6, -8}));
        EXPECT_TRUE((qa / 2) == Quaternion(1, {-1, 1.5, -2}));
        EXPECT_FLOAT_EQ(qa.Modulus(), sqrt(33));
        EXPECT_FLOAT_EQ(qb.Modulus(), sqrt(66));
        EXPECT_TRUE(qa.Normalized() == (qa / sqrt(33)));
        EXPECT_TRUE(qb.Normalized() == (qb / sqrt(66)));
        EXPECT_TRUE((qa * qb) == Quaternion(-41, {-4, 9, -20}));
        EXPECT_TRUE((qb * qa) == Quaternion(-41, {-8, 17, -12}));
        EXPECT_TRUE((Quaternion(1, {0, 0, 0}) / qa) == (Quaternion(2, {2, -3, 4}) / 33));
    }

    {
        const double PI = 3.141592653589793;
        BVec v(1 / sqrt(2), 0, 1 / sqrt(2));
        Quaternion q(v, PI / 2);
        Quaternion p(0, {2, 0, 0});

        // 逆时针绕 v 转 90 度
        EXPECT_TRUE((q * p / q) == Quaternion(0, BVec(1, sqrt(2), 1)));

        // 从一个轴转到另一个轴
        BVec v1({2.4, 3.6, 5.8});
        v1.Normalize();
        BVec v2({5.1, 9.6, -2.7});
        v2.Normalize();
        Quaternion q2(v1, v2);
        EXPECT_TRUE((q2 * Quaternion(0, v1) / q2) == Quaternion(0, v2));
    }
}

// 测试 Vector
TEST(TestBVector, BasisFunctions)
{
    // 测试等于
    {
        EXPECT_TRUE(Vector({1, 2, 3}) == Vector({1, 2, 3}));
        EXPECT_TRUE(Vector({1.5, 2.6, 3.8, 4.8, 5.7}) == Vector({1.5, 2.6, 3.8, 4.8, 5.7}));
        EXPECT_FALSE(Vector({1.5, 2.6, 3.8, 4.8, 5.7}) == Vector({1, 2, 3}));
    }

    // 测试构造
    {
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        EXPECT_TRUE(v.Size() == 4);

        Vector v2(v.Size(), 1.1);
        EXPECT_TRUE(v2 == Vector({1.1, 1.1, 1.1, 1.1}));

        Vector v3 = v2;
        EXPECT_TRUE(v3 == Vector({1.1, 1.1, 1.1, 1.1}));
    }

    // 测试赋值
    {
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        Vector v2(4);
        v2 = v;
        EXPECT_TRUE(v2 == v);
    }

    // 测试 []
    {
        double arr[] = {1.5, 2.6, 3.8, 4.7};

        // 测试 []
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v[i] == arr[i]);

        // 测试取引用
        for (int i = 0; i < v.Size(); i++)
            v[i] = i;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v[i] == i);
    }

    // 测试四则运算
    {
        double arr[] = {1.5, 2.6, 3.8, 4.7};
        double arr2[4];
        for (int i = 0; i < 4; i++)
            arr2[i] = arr[i] * 2;

        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        Vector v2 = v + v;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v2[i] == arr2[i]);

        Vector v3 = v - v;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v3[i] == 0);

        Vector v4 = v * 2;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v4[i] == arr2[i]);

        Vector v5 = v2 / 2;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v5[i] == arr[i]);

        Vector v6 = -v2;
        for (int i = 0; i < v.Size(); i++)
            EXPECT_TRUE(v6[i] == -arr2[i]);
    }

    // 测试乘法
    {
        Vector v({1, 2, 3});
        Matrix A(1, 7, {1, 2, 3, 4, 5, 6, 7});
        Matrix B = v * A;
        EXPECT_TRUE(B == Matrix(3, 7, {1, 2, 3, 4, 5, 6, 7, 2, 4, 6, 8, 10, 12, 14, 3, 6, 9, 12, 15, 18, 21}));
    }
}

// 测试 Vector 算法
TEST(TestBVector, Algorithm)
{
    // 测试排序
    {
        Vector X({5, 2.0, 5.1, 9.3, 1.5, -5, 6.4, 3.9, 4.2});
        X.Sort();
        EXPECT_TRUE(X == Vector({-5, 1.5, 2.0, 3.9, 4.2, 5, 5.1, 6.4, 9.3}));
    }

    // 测试求和
    {
        Vector X({1, 2, 3, 4, 5, 6});
        EXPECT_TRUE(X.Sum() == 21);
    }

    // 测试绝对值
    {
        Vector X({-1, -2, -3, -4, -5, -6});
        Vector Y({1, 2, 3, 4, 5, 6});
        EXPECT_TRUE(X.Abs() == Y);
    }

    // 测试符号
    {
        Vector X({-1, 2, -3, 4, -5, -6});
        EXPECT_TRUE(X.Sgn() == Vector({-1, 1, -1, 1, -1, -1}));
    }

    // 测试范数
    {
        Vector x({1, 2, 3, 4, 5, 6});
        EXPECT_TRUE(x.Norm(0) == 6);
        EXPECT_TRUE(x.Norm(1) == 21);
        EXPECT_TRUE(x.Norm(2) == sqrt(91));
    }

    // 最值
    {
        Vector x({1, 2, 33, -46, 5, 6});
        EXPECT_TRUE(x.Min() == -46);
        EXPECT_TRUE(x.Max() == 33);
        EXPECT_TRUE(x.MaxId() == 3);
    }

    // 转换为矩阵
    {
        Vector x({1, 2, 33, -46, 5, 6});
        Matrix A1(1, 6, {1, 2, 33, -46, 5, 6});
        Matrix A2(6, 1, {1, 2, 33, -46, 5, 6});
        EXPECT_TRUE(A2 == x.ToBMatrix());
        EXPECT_TRUE(A1 == x.Transpose());
    }
}

// 测试 Matrix
TEST(TestBMatrix, BasisFunctions)
{
    // 测试比较
    {
        EXPECT_TRUE(Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_TRUE(Matrix(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Matrix(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_FALSE(Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                     Matrix(3, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9}));
    }

    // 测试初始化
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE((A.Row() == 3 && A.Col() == 4));

        Matrix A2(4, 3, 1.1);
        EXPECT_TRUE(A2 == Matrix(4, 3, {1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1}));

        Matrix A3 = A2;
        EXPECT_TRUE(A3 == A2);

        Matrix A4 = Matrix::Identity(5);
        Matrix A5(5, 5);
        for (int i = 0; i < 5; i++)
            A5(i, i) = 1;
        EXPECT_TRUE(A4 == A5);
    }

    // 测试赋值
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A2(3, 4);
        A2 = A;
        EXPECT_TRUE(A2 == A);
    }

    // 测试 ()
    {
        double arr[] = {1.2, 2.5, 34, 4.8, 5.1, 6.9, 7.1, 8.2, 9.7, 10.5, 11.4, 12.2};

        // 测试取引用
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        for (int i = 0; i < A.Row(); i++)
            for (int j = 0; j < A.Col(); j++)
                A(i, j) = arr[i * 4 + j];
        for (int i = 0; i < A.Row(); i++)
            for (int j = 0; j < A.Col(); j++)
                EXPECT_TRUE((A(i, j) == arr[i * 4 + j]));

        EXPECT_TRUE(A.Row() == 3);
        EXPECT_TRUE(A.Col() == 4);
    }

    // 测试四则运算
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A2 = A * 2;
        EXPECT_TRUE(A2 == Matrix(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        Matrix A3 = A2 / 2;
        EXPECT_TRUE(A3 == A);

        Matrix A4 = A + A;
        EXPECT_TRUE(A4 == A2);

        Matrix A5 = A2 - A;
        EXPECT_TRUE(A5 == A);

        Vector v({1, 2, 3, 4});
        Vector v2 = A * v;
        EXPECT_TRUE(v2 == Vector({30, 70, 110}));

        Matrix B(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A6 = A * B;
        EXPECT_TRUE(A6 == Matrix(3, 3, {70, 80, 90, 158, 184, 210, 246, 288, 330}));

        Matrix A7 = -A;
        EXPECT_TRUE((A7 + A) == Matrix(3, 4));

        srand(time(0));
        Matrix A8 = Matrix::Identity(5);
        for (int i = 0; i < 20; i++)
        {
            A8 = A8 * Matrix::ExchangeRC(5, rand() % 5, rand() % 5);
            A8 = A8 * Matrix::TimesRC(5, rand() % 5, rand() % 3 + 1);
            A8 = A8 * Matrix::TimesAddRC(5, rand() % 5, rand() % 5, rand() % 3 + 1);
        }

        auto C = A8 * A8 * A8;
        auto D = A8 ^ 3;
        auto E = C - D;
        // for (int i = 0; i < C.Row(); i++)
        //{
        //     for (int j = 0; j < C.Col(); j++)
        //         printf("%f ", C(i, j));
        //     printf("\n");
        // }
        // printf("\n");

        // for (int i = 0; i < D.Row(); i++)
        //{
        //     for (int j = 0; j < D.Col(); j++)
        //         printf("%f ", D(i, j));
        //     printf("\n");
        // }
        // printf("\n");

        // for (int i = 0; i < E.Row(); i++)
        //{
        //     for (int j = 0; j < E.Col(); j++)
        //         printf("%f ", E(i, j));
        //     printf("\n");
        // }

        // int flag = 0;
        // printf("%f\n", ((A8.Invert(&flag)) - (A8 ^ -1)).Norm());
        // printf("%f\n", ((A8 * A8 * A8) - (A8 ^ 3)).Norm());
        // printf("flag:%d\n", flag);

        EXPECT_TRUE(Matrix::Identity(5) == (A8 ^ 0));
        EXPECT_TRUE(A8 == (A8 ^ 1));
        EXPECT_TRUE((A8 * A8) == (A8 ^ 2));
        EXPECT_TRUE((A8 * A8 * A8) == (A8 ^ 3));
        EXPECT_TRUE((A8.Invert()) == (A8 ^ -1));
        EXPECT_TRUE((A8.Invert() * A8.Invert() * A8.Invert()) == (A8 ^ -3));
    }
}

// 测试 Matrix
TEST(TestBMatrix, OpFunctions)
{
    // 范数
    {
        Matrix A(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE(A.Norm(0) == 33);
        EXPECT_TRUE(A.Norm(1) == 30);

        EXPECT_DOUBLE_EQ(A.Norm(2), 25.462407436036393);
        EXPECT_DOUBLE_EQ(A.Norm('f'), 25.495097567963924);
    }

    // 条件数
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        EXPECT_DOUBLE_EQ(A.Condition(), 133.0);
    }

    // 行列式
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        EXPECT_DOUBLE_EQ(A.Det(), -2.9999999999999973);
    }

    // 测试迹
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        EXPECT_DOUBLE_EQ(A.Trace(), 16);
    }

    // 取元素
    {
        Matrix A(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE(A.Upper() == Matrix(4, 3, {1, 2, 3, 0, 5, 6, 0, 0, 9, 0, 0, 0}));
        EXPECT_TRUE(A.Lower() == Matrix(4, 3, {1, 0, 0, 4, 5, 0, 7, 8, 9, 10, 11, 12}));

        Matrix B(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE(B.Upper() == Matrix(3, 4, {1, 2, 3, 4, 0, 6, 7, 8, 0, 0, 11, 12}));
        EXPECT_TRUE(B.Lower() == Matrix(3, 4, {1, 0, 0, 0, 5, 6, 0, 0, 9, 10, 11, 0}));

        EXPECT_TRUE(A.Transpose() == Matrix(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
    }

    // 求逆
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Matrix B = A.Invert();
        EXPECT_TRUE(B == Matrix(3, 3, {-2.0 / 3, -2.0 / 3, 1.0, -4.0 / 3, 11.0 / 3, -2.0, 1.0, -2.0, 1.0}));
    }

    // LU 分解
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Matrix B = A.LU();
        EXPECT_TRUE(B == Matrix(3, 3, {1, 4, 7, 2, -3, -6, 3, 2, 1}));
    }

    // Cholesky 分解
    {
        Matrix A(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});
        Matrix B = A.Cholesky();
        EXPECT_TRUE(B == Matrix(4, 4, {2, -1, 2, 1, -1, 3, 0, -2, 2, 0, 2, 1, 1, -2, 1, 1}));

        Matrix C = A.CholeskyD();
        EXPECT_TRUE(C == Matrix(4, 4, {4, -0.5, 1, 0.5, -0.5, 9, 0, -2.0 / 3, 1, 0, 4, 0.5, 0.5, -2.0 / 3, 0.5, 1}));
    }
    // 其余函数通过测试求解器间接验证
}

// 测试 Sparse
TEST(TestBSparse, BasisFunctions)
{
    // 测试初始化
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(A(i, j) == SA(i, j));

        Matrix B(4, 8);
        Sparse SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB.Set(j, i, value);
        }

        EXPECT_TRUE(SA.Row() == 8);
        EXPECT_TRUE(SA.Col() == 4);

        Sparse SB2(4, 8);
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                SB2.Set(j, i, 1);
        EXPECT_TRUE(SB2.Nnz() == 32);
    }

    // 测试加减
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        Matrix B(8, 4);
        Sparse SB(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(i, j) = value;
            SB.Set(i, j, value);
        }

        Matrix C = A + B;
        Sparse SC = SA + SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(C(i, j) == SC(i, j));

        C = A - B;
        SC = SA - SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(C(i, j) == SC(i, j));
    }

    // 测试数乘除
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        Matrix C = A / 3;
        Sparse SC = SA / 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(C(i, j) == SC(i, j));

        C = A * 3;
        SC = SA * 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(C(i, j) == SC(i, j));
    }

    // 测试乘矩阵
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        Matrix B(4, 8);
        Sparse SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB.Set(j, i, value);
        }

        Matrix C = A * B;
        Sparse SC = SA * SB;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 8; j++)
                EXPECT_TRUE(C(i, j) == SC(i, j));

        Matrix D = B * A;
        Sparse SD = SB * SA;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(D(i, j) == SD(i, j));
    }

    // 测试乘向量
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        Vector B(4);
        for (int k = 0; k < 4; k++)
            B[k] = rand() % 10;

        Vector C = A * B;
        Vector SC = SA * B;
        for (int i = 0; i < 8; i++)
            EXPECT_TRUE(C[i] == SC[i]);
    }

    // 测试转换为矩阵
    {
        srand(time(0));

        Matrix A(8, 4);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.Set(i, j, value);
        }

        Matrix B = SA.ToBMatrix();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_TRUE(A(i, j) == B(i, j));
    }

    // 测试转置
    {
        srand(time(0));

        Matrix A(4, 8);
        Sparse SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(j, i) = value;
            SA.Set(i, j, value);
        }

        Sparse B = SA.Transpose();
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 8; j++)
                EXPECT_TRUE(A(i, j) == B(i, j));
    }
}

// 下三角
TEST(TestLinearSolver, Lower)
{
    Matrix A(3, 3, {1, 0, 0, 2, 5, 0, 3, 6, 10});
    Vector b({1, 3, 4});
    LinearSolver Solver;
    Vector x = Solver.SolveLower(A, b);
    EXPECT_TRUE(x == Vector({1, 0.2, -0.02}));

    x = Solver.SolveLower(A, b, true);
    EXPECT_TRUE(x == Vector({1, 1, -5}));
}

// 上三角
TEST(TestLinearSolver, Upper)
{
    Matrix A(3, 3, {1, 3, 4, 0, 5, 6, 0, 0, 10});
    Vector b({1, 2, 4});
    LinearSolver Solver;
    Vector x = Solver.SolveUpper(A, b);
    EXPECT_TRUE(x == Vector({-0.36, -0.08, 0.4}));

    x = Solver.SolveUpper(A, b, true);
    EXPECT_TRUE(x == Vector({51, -22, 4}));
}

// 基本求解
TEST(TestLinearSolver, Solving)
{
    Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
    Vector b({1, 2, 4});
    LinearSolver Solver;
    Vector x = Solver.Solve(A, b);
    EXPECT_TRUE(x == Vector({2.0, -2.0, 1.0}));

    x = Solver.Solve(A, b, true);
    EXPECT_TRUE(x == Vector({2.0, -2.0, 1.0}));
}

// 对称求解
TEST(TestLinearSolver, SymSolving)
{
    Matrix A(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});
    Vector b({8, -1, 14, 6});
    LinearSolver Solver;
    Vector x = Solver.Cholesky(A, b);
    EXPECT_TRUE(x == Vector({1, 1, 1, 1}));

    x = Solver.Cholesky(A, b, true);
    EXPECT_TRUE(x == Vector({1, 1, 1, 1}));

    x = Solver.Solve(A, b);
    EXPECT_TRUE(x == Vector({1, 1, 1, 1}));

    x = Solver.Solve(A, b, true);
    EXPECT_TRUE(x == Vector({1, 1, 1, 1}));

    Solver.CG(A, b, x);
    EXPECT_TRUE(x == Vector({1, 1, 1, 1}));
}

// Jacobi 迭代
TEST(TestLinearSolver, Jacobi)
{
    Matrix A(3, 3, {1, 2, -2, 1, 1, 1, 2, 2, 1});
    Vector b({-1, 6, 9});
    LinearSolver Solver;
    Vector x = Solver.Jacobi(A, b);
    EXPECT_TRUE(x == Vector({1, 2, 3}));
}

// GS 迭代
TEST(TestLinearSolver, GS)
{
    Matrix A(3, 3, {2, -1, 1, 1, 1, 1, 1, 1, -2});
    Vector b({3, 6, -3});
    LinearSolver Solver;
    Vector x = Solver.GaussSediel(A, b);
    EXPECT_TRUE(x == Vector({1, 2, 3}));
}

// SOR 迭代
TEST(TestLinearSolver, SOR)
{
    Matrix A(3, 3, {2, -1, 1, 1, 1, 1, 1, 1, -2});
    Vector b({3, 6, -3});
    LinearSolver Solver;
    Vector x = Solver.SOR(A, b, 0.8);
    EXPECT_TRUE(x == Vector({1, 2, 3}));

    x = Solver.SOR(A, b, 1.2);
    EXPECT_TRUE(x == Vector({1, 2, 3}));
}

// 最小二乘
TEST(TestLinearSolver, LSQ)
{
    Matrix A(7, 4, {1, 2, -2, 5, 1, 1, 1, 4, 2, 2, 2, 9, 2, 3, 4, 5, 6, 8, 9, -5, 2, 3, 4, 5, 6, 7, 9, -5});
    Vector b({5, 6, 7, 8, 1, 2, 4});

    LinearSolver Solver;
    Vector x = Solver.QR(A, b);
    EXPECT_TRUE(x == Vector({2.479465943383723, -0.880183882547382, -0.264909586025962, 0.665423885233720}));
    // for (int i = 0; i < x.Size(); i++)
    //     printf("%f ", x[i]);
    // printf("\n");

    Vector x1(4);
    Solver.CG(A.Transpose() * A, A.Transpose() * b, x1);
    EXPECT_TRUE(x1 == Vector({2.479466, -0.880184, -0.264910, 0.665424}));
    // for (int i = 0; i < x1.Size(); i++)
    //     printf("%f ", x1[i]);
    // printf("\n");
    //   printf("norm:%f\n", (A * x1 - b).Norm());

    // 最小二乘解
    //{
    //    Eigen::MatrixXd B(7, 4);
    //    for (int i = 0; i < 7; i++)
    //        for (int j = 0; j < 4; j++)
    //            B(i, j) = A(i, j);
    //    Eigen::VectorXd C(7);
    //    for (int i = 0; i < 7; i++)
    //        C[i] = b[i];
    //    Eigen::VectorXd x = B.colPivHouseholderQr().solve(C);
    //    for (int j = 0; j < 4; j++)
    //        printf("%f ", x[j]);
    //    printf("\n");
    //}
}

// 幂法
TEST(TestLinearSolver, Power)
{
    Matrix A(5, 5, {-15, 85, -225, 274, -120, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0});
    LinearSolver Solver;
    EXPECT_DOUBLE_EQ(Solver.Power(A, 1), -5.000000000004504);
}

// 反幂法
TEST(TestLinearSolver, InvPower)
{
    Matrix A(5, 5, {-15, 85, -225, 274, -120, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0});
    LinearSolver Solver;
    Vector x = Solver.InversePower(A, -5);
    EXPECT_TRUE(x.Abs() == Vector({0.979795947278826, 0.195959189455760, 0.039191837891151, 0.007838367578230,
                                   0.001567673515646}));
}

// 隐式 QR 分解，存在问题
TEST(TestLinearSolver, ImplicitQR)
{
    int n = 16;
    // Matrix A(n, n, {1.1908,  -1.0565, -2.1707, 0.5913,  0,       0.7310,  -1.2025, 1.4151, -0.0592,
    //                  -0.6436, -0.3179, 0.5779,  -0.0198, -0.8051, -1.0106, 0.3803,  1.0950, 0.0403,
    //                  -0.1567, 0.5287,  0.6145,  -1.0091, -1.8740, 0.6771,  -1.6041, 0.2193, 0.5077,
    //                  -0.0195, 0.4282,  0.5689,  0.2573,  -0.9219, 1.6924,  -0.0482, 0.8956, -0.2556});

    srand(time(0));
    Matrix A(n, n);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < n; j++)
            A(i, j) = rand() % 10 - 5;

    Eigen::MatrixXd EA(n, n);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < n; j++)
            EA(i, j) = A(i, j);

    LinearSolver Solver;
    Matrix Q(n, n);
    for (int i = 0; i < n; i++)
        Q(i, i) = 1;

    // 验证 Hessenberg
    Matrix H = A.Hessenberg(&Q);
    EXPECT_TRUE(A == Matrix(Q * H * Q.Transpose()));

    Matrix P(n, n);
    for (int i = 0; i < n; i++)
        P(i, i) = 1;
    Matrix S = Solver.ImplicitQR(A, &P);

    // 特征分解，得到复特征值和特征向量
    Eigen::EigenSolver<Eigen::MatrixXd> es(EA);

    // 获得 eigen 计算出的特征值及行列式
    std::vector<double> eigen_eigen;
    Eigen::VectorXcd cval = es.eigenvalues();
    for (int i = 0; i < cval.size(); i++)
    {
        if (fabs(cval[i].imag()) > 1e-8)
        {
            // eigen_eigen.push_back(cval[i].real() * cval[i].real() + cval[i].imag() * cval[i].imag());
            i++;
        }
        else
            eigen_eigen.push_back(cval[i].real());
        // printf("%f,%f\n", cval[i].real(), cval[i].imag());
    }
    std::sort(eigen_eigen.begin(), eigen_eigen.end());

    // 获得当前算法计算出的特征值及行列式
    std::vector<double> now_eigen;
    for (int i = 0; i < S.Row(); i++)
    {
        // 此时处于非特征值位置
        if (i < (S.Row() - 1) && fabs(S(i + 1, i)) > 1e-5)
        {
            // now_eigen.push_back(S(i, i) * S(i + 1, i + 1) - S(i, i + 1) * S(i + 1, i));
            i++;
        }
        else
            now_eigen.push_back(S(i, i));
    }
    std::sort(now_eigen.begin(), now_eigen.end());

    /*   for (int i = 0; i < eigen_eigen.size(); i++)
           printf("%f ", eigen_eigen[i]);
       printf("\n");
       for (int i = 0; i < now_eigen.size(); i++)
           printf("%f ", now_eigen[i]);*/

    // EXPECT_TRUE(eigen_eigen == now_eigen);

    // printf("\n");
    // for (int i = 0; i < n; i++)
    //{
    //     for (int j = 0; j < n; j++)
    //         printf("%f ", S(i, j));
    //     printf("\n");
    // }

    // 由于 QR 方法过程中去除了很多相对小的元素，导致最终计算结果会有一定误差
    // EXPECT_LE((A - P * S * P.Transpose()).Norm(), 1e-6);
}

// 二分法
TEST(TestLinearSolver, Bisection)
{
    // 针对三对角阵
    Matrix A(4, 4, {1, 1, 0, 0, 1, 2, 3, 0, 0, 3, 6, 10, 0, 0, 10, 20});
    LinearSolver Solver;
    Vector lambda = Solver.Bisection(A);
    EXPECT_TRUE(lambda == Vector({-1.523734777993481, 0.974969844946525, 4.258610702322208, 25.290154230724745}));
}

// 过关 Jacobi
TEST(TestLinearSolver, PassJacobi)
{
    Matrix A(4, 4, {1, 1, 1, 1, 1, 2, 3, 4, 1, 3, 6, 10, 1, 4, 10, 20});
    LinearSolver Solver;
    Matrix Q = Solver.PassJacobi(A, 10);
    EXPECT_TRUE(Q == Matrix(4, 4,
                            {0.787275376005945, -0.308686320226581, 0.530365719772128, 0.060186720547497,
                             -0.163233650892682, 0.723090316194097, 0.640331730884963, 0.201172672757594,
                             -0.532106691666762, -0.594550779583976, 0.391832131205882, 0.458082328991412,
                             0.265357732569444, 0.168411759765698, -0.393897269179563, 0.863752102325145}));
    EXPECT_TRUE(A.Diag() == Vector({0.453834550025667, 0.038016015229141, 2.203446167647325, 26.304703267097874}));
}

// 对称求解
TEST(TestLinearSolver, SymImplicitQR)
{
    Matrix A(4, 4, {1, 1, 1, 1, 1, 2, 3, 4, 1, 3, 6, 10, 1, 4, 10, 20});
    LinearSolver Solver;
    Matrix Q(4, 4);
    for (int i = 0; i < 4; i++)
        Q(i, i) = 1;
    Matrix S = Solver.ImplicitQR(A, &Q, true);
    EXPECT_TRUE(Q == Matrix(4, 4,
                            {0.060186720547497, -0.530365719772128, 0.787275376005945, -0.308686320226581,
                             0.201172672757594, -0.640331730884963, -0.163233650892682, 0.723090316194097,
                             0.458082328991412, -0.391832131205882, -0.532106691666762, -0.594550779583976,
                             0.863752102325145, 0.393897269179563, 0.265357732569444, 0.168411759765698}));
    EXPECT_TRUE(S.Diag() == Vector({26.304703267097874, 2.203446167647325, 0.453834550025667, 0.038016015229141}));
}

// 奇异值分解
TEST(TestLinearSolver, SVD)
{
    Matrix A(7, 4, {1, 2, -2, 5, 1, 1, 1, 4, 2, 2, 2, 9, 2, 3, 4, 5, 6, 8, 9, -5, 2, 3, 4, 5, 6, 7, 9, -5});
    LinearSolver Solver;

    // 注意先设为对角阵
    Matrix U(7, 7);
    for (int i = 0; i < 7; i++)
        U(i, i) = 1;
    Matrix V(4, 4);
    for (int i = 0; i < 4; i++)
        V(i, i) = 1;

    Matrix S = Solver.SVD(A, &U, &V);
    EXPECT_TRUE(S == Matrix(7, 4,
                            {20.647924190113297,
                             0,
                             0,
                             0,
                             0,
                             14.679570743204255,
                             0,
                             0,
                             0,
                             0,
                             0.764867795802106,
                             0,
                             0,
                             0,
                             0,
                             2.930598350088416,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0,
                             0}));
    EXPECT_TRUE(A == Matrix(U * S * V.Transpose()));
}
